#include "Aria.h"
#include "RobotController.h"
#include <iostream>
#include <string>
// ip robot 192.168.43.168
using namespace std;
int main (int argc, char* argv[]){
	//Inizializzo la libreria

	Aria::init();

	//Definisco un robot
	ArRobot robot;
	//Analisi degli argomenti e connessione
	ArArgumentParser parser(&argc, argv);
	ArSimpleConnector connector(&parser);

	//Connessione con il robot

	parser.loadDefaultArguments();
	if(!connector.parseArgs()){
		cout<<"Errore: impostazioni sconoscite! Il programma verrà terminato!";
		Aria::exit(0);
		exit(1);
	}if(!connector.connectRobot(&robot)){
		cout<<"Errore: impossibile avviare la connessione! Il programma verrà terminato!";
		Aria::exit(0);
		exit(1);
	}

	//Prendo il lock sul robot ed inserisco la configurazione
	robot.lock();
	cout<<"\n...Connessione al robot stabilita!...\n";
	//Avvio i motori
	robot.comInt(ArCommands::ENABLE,1);
	robot.unlock();
	//Avvio il sonar
	ArSonarDevice sonar;
	robot.addRangeDevice(&sonar);


	RobotController controller(&robot);
	controller.run();

	//Rilascio il robot
	//robot.unlock();
	robot.run(true);
	Aria::shutdown();
	return 0;

}

